#! /usr/bin/env python

# Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
# You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
# 
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of ETHZ-ASL nor the
# names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


PACKAGE='asctec_hl_interface'

#from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# @todo Think about levels. Setting most of these to STOP to guarantee atomicity.
# @todo Double check these ranges, defaults

ENABLE_CTRL         = gen.const("ENABLE_CTRL",      int_t, 0x00000001, "enable_ctrl")
PACKET_RATE         = gen.const("PACKET_RATE",      int_t, 0x00000002, "configures the packet rates")
HLI_CONFIG          = gen.const("HLI_CONFIG",       int_t, 0x00000004, "configures where control and state estimation are performed")
MISC                = gen.const("MISC",             int_t, 0x01000000, "misc")


enum_position_ctrl_mode = gen.enum([
gen.const("POSCTRL_OFF",             str_t, "off",         "position control off"),
gen.const("POSCTRL_HIGHLEVEL",       str_t, "HighLevel",   "position control runs on the HLP."), 
gen.const("POSCTRL_GPS",             str_t, "GPS",         "gps position control on the LLP.")],
"An enum to set the position control mode.")

enum_state_estimation_mode = gen.enum([
gen.const("STATE_EST_OFF",           str_t, "off",         "state estimation off"),
gen.const("STATE_EST_HIGHLEVEL_SSDK",str_t, "HighLevel_SSDK","state estimation runs on the HLP within the SSDK."), 
gen.const("STATE_EST_HIGHLEVEL_EKF", str_t, "HighLevel_EKF","state estimation runs on the HLP within an EKF."), 
gen.const("STATE_EST_EXTERN",        str_t, "Extern",      "state estimation is performed externally.")],
"An enum to set the state estimation mode.")


#       Name                        Type        Reconfiguration level    Description                                           Default Min     Max
gen.add("packet_rate_imu",          double_t,   PACKET_RATE["value"],    "configures packet rate [Hz] of imu packet.",         50.0,   0.0,    1000.0)
gen.add("packet_rate_rc",           double_t,   PACKET_RATE["value"],    "configures packet rate [Hz] of rc packet.",          10.0,   0.0,    1000.0)
gen.add("packet_rate_gps",          double_t,   PACKET_RATE["value"],    "configures packet rate [Hz] of gps packet.",         5.0,    0.0,    1000.0)
gen.add("packet_rate_ssdk_debug",   double_t,   PACKET_RATE["value"],    "configures packet rate [Hz] of ssdk debug packet.",  5.0,    0.0,    1000.0)
gen.add("packet_rate_ekf_state",    double_t,   PACKET_RATE["value"],    "configures packet rate [Hz] of ekf state packet.",   0.0,    0.0,    1000.0)
gen.add("packet_rate_mag",          double_t,   PACKET_RATE["value"],    "configures packet rate [Hz] of magnetic compass packet.",   0.0,    0.0,    1000.0)

gen.add("battery_warning",          double_t,   HLI_CONFIG["value"],     "Sets the battery warning voltage." ,                 9.8,    8.0,      20.0)
gen.add("enable_x",                 bool_t,     HLI_CONFIG["value"],     "enables x-control. ",                                True)
gen.add("enable_y",                 bool_t,     HLI_CONFIG["value"],     "enables y-control. ",                                True)
gen.add("enable_z",                 bool_t,     HLI_CONFIG["value"],     "enables z-control. ",                                True)
gen.add("enable_yaw",               bool_t,     HLI_CONFIG["value"],     "enables yaw-control. ",                              True)

gen.add("position_control",         str_t,      HLI_CONFIG["value"],     "Enables position control either on the HLP or GPS based position control on the LLP." ,"off", edit_method=enum_position_ctrl_mode)
gen.add("state_estimation",         str_t,      HLI_CONFIG["value"],     "Determines where state estimation is performed." ,"off", edit_method=enum_state_estimation_mode)

gen.add("max_velocity_xy",          double_t,   MISC["value"],           "maximum velocity in x/y [m/s].",                     0.5,    0.,     10.0)
gen.add("max_velocity_z",           double_t,   MISC["value"],           "maximum velocity in z [m/s].",                       0.5,    0.,     2.0)
gen.add("max_velocity_yaw",         double_t,   MISC["value"],           "maximum velocity yaw [rad/s].",                      0.1,    0.,     1.57)
gen.add("min_pos_x",                double_t,   MISC["value"],           "lower bound for position x [m].",                   -1.0e3, -1.0e3, 1.0e3)
gen.add("min_pos_y",                double_t,   MISC["value"],           "lower bound for position y [m].",                   -1.0e3, -1.0e3, 1.0e3)
gen.add("min_pos_z",                double_t,   MISC["value"],           "lower bound for position z [m].",                   -1.0e3, -1.0e3, 1.0e3)
gen.add("max_pos_x",                double_t,   MISC["value"],           "upper bound for position x [m].",                   1.0e3,  -1.0e3, 1.0e3)
gen.add("max_pos_y",                double_t,   MISC["value"],           "upper bound for position y [m].",                   1.0e3,  -1.0e3, 1.0e3)
gen.add("max_pos_z",                double_t,   MISC["value"],           "upper bound for position z [m].",                   1.0e3,  -1.0e3, 1.0e3)


exit(gen.generate(PACKAGE, "Config", "HLInterface"))
